
#include "WorldEditor.h"
#include "ode/collision.h"
#include "ode/internal/objects.h"
#include "ode/contact.h"
#include "../MT/Math3D.h"

	void WorldEditor::onMouseUp(int x, int y)
	{
		mDraggingBody = nullptr;
	}

	void WorldEditor::onMouseDown(int x, int y)
	{	
		mDraggingBody = getSelectedBody(x, y);	
	}

	void WorldEditor::alignBodyOnGeometry(dxBody *body, int x, int y)
	{
		dGeomID rayID = dCreateRay(nullptr, 100);

		ors::Vector rayOrigin;
		ors::Vector rayDir;
		mCamera->getCameraRay(x, y, rayOrigin,  rayDir);
		dGeomRaySet(rayID, rayOrigin.p[0], rayOrigin.p[1], rayOrigin.p[2], rayDir.p[0], rayDir.p[1], rayDir.p[2]);

		dContactGeom contactGeoms[1];
		int skip = (int)sizeof(contactGeoms[0]);
		double closestDist = 999999999;
		ors::Vector closestHit;
		bool hit = false;
		for (unsigned int i = 0; i < mBodies->N; i++)
		{
			if ((*mBodies)(i) != body && dCollide((*mBodies)(i)->geom, rayID, 1, contactGeoms, skip) > 0)
			{
				ors::Vector hitPos(contactGeoms[0].pos[0], contactGeoms[0].pos[1], contactGeoms[0].pos[2]);
				double dist = (hitPos - rayOrigin).lengthSqr();
				if (dist < closestDist)
				{
					hit = true;
					closestDist = dist;
					closestHit = hitPos;
				}
			}
		}

		if (hit)
		{
			dReal aabb[6];
			dGeomGetAABB(body->geom, aabb);
			float offset = (aabb[3] - aabb[2]) * 0.5;
			closestHit.p[2] += offset;
			((ors::Body*)body->userdata)->X.pos = closestHit;
		}
	}

	void WorldEditor::onMouseMove(int x, int y)
	{
		if (mDraggingBody) alignBodyOnGeometry(mDraggingBody, x, y);
	}

	dxBody* WorldEditor::getSelectedBody(int x, int y)
	{
		dGeomID rayID = dCreateRay(nullptr, 100);

		ors::Vector rayOrigin;
		ors::Vector rayDir;
		mCamera->getCameraRay(x, y, rayOrigin,  rayDir);
		dGeomRaySet(rayID, rayOrigin.p[0], rayOrigin.p[1], rayOrigin.p[2], rayDir.p[0], rayDir.p[1], rayDir.p[2]);

		dContactGeom contactGeoms[1];
		int skip = (int)sizeof(contactGeoms[0]);
		double closestDist = 99999999;
		dxBody *closestBody = nullptr;
		for (int i = 0; i < mBodies->N; i++)
		{
			if (dCollide((*mBodies)(i)->geom, rayID, 1, contactGeoms, skip) > 0)
			{
				ors::Vector hitPos(contactGeoms[0].pos[0], contactGeoms[0].pos[1], contactGeoms[0].pos[2]);
				double dist = (hitPos - rayOrigin).lengthSqr();
				if (!closestBody || dist < closestDist)
				{
					closestDist = dist;
					closestBody = (*mBodies)(i);
				}
			}
		}

		return closestBody;
	}